#include "motor.h"
#include "os.h"
#include "bsp_adc.h"
#include "components.h"
#include "stm32f1xx_hal_gpio.h"


void Motor_task(void * arg) {
	PWM_SetFrequency(&pwm2[1],50);
	PWM_SetFrequency(&pwm2[2],50);
	PWM_SetFrequency(&pwm2[3],50);
	PWM_SetFrequency(&pwm2[4],50);
while(1){
	if(control.triSwitch[0]==3){
		if(control.channel[1]==0)
		{
		PWM_SetDutyRatio(&pwm2[1],0);
		PWM_SetDutyRatio(&pwm2[2],0);
		}
		if(control.channel[1]<0){
		PWM_SetDutyRatio(&pwm2[1],-control.channel[1]); 
		PWM_SetDutyRatio(&pwm2[2],0);		
		}
		if(control.channel[1]>0){
		PWM_SetDutyRatio(&pwm2[1],0);
		PWM_SetDutyRatio(&pwm2[2],control.channel[1]);
		}
		if(control.channel[3]==0)
		{
		PWM_SetDutyRatio(&pwm2[3],0.5);
		PWM_SetDutyRatio(&pwm2[4],0.5);
		}
		if(control.channel[3]<0){
		PWM_SetDutyRatio(&pwm2[3],-control.channel[3]);
		PWM_SetDutyRatio(&pwm2[4],0);		
		} 
		if(control.channel[3]>0){
		PWM_SetDutyRatio(&pwm2[3],0);
		PWM_SetDutyRatio(&pwm2[4],control.channel[3]);
		}
	}
		
	if(control.triSwitch[0]==1){
		if((adc_data[2].data + adc_data[2].last_data)/2>400){
			if((adc_data[0].data + adc_data[0].last_data)/2>=850 && (adc_data[1].data + adc_data[1].last_data)/2 >=350){
				PWM_SetDutyRatio(&pwm2[1],0);
				PWM_SetDutyRatio(&pwm2[2],0.7);
				PWM_SetDutyRatio(&pwm2[3],0);
				PWM_SetDutyRatio(&pwm2[4],0.7);
			}
			if((adc_data[0].data + adc_data[0].last_data)/2>=850 && (adc_data[1].data + adc_data[1].last_data)/2<350){
				PWM_SetDutyRatio(&pwm2[1],0);
				PWM_SetDutyRatio(&pwm2[2],0.7);
				PWM_SetDutyRatio(&pwm2[3],0);
				PWM_SetDutyRatio(&pwm2[4],0);
			}
			if((adc_data[0].data + adc_data[0].last_data)/2<850 && (adc_data[1].data + adc_data[1].last_data)/2>=350){
				PWM_SetDutyRatio(&pwm2[1],0);
				PWM_SetDutyRatio(&pwm2[2],0);
				PWM_SetDutyRatio(&pwm2[3],0);
				PWM_SetDutyRatio(&pwm2[4],0.7);
			}
			if((adc_data[0].data + adc_data[0].last_data)/2<850 && (adc_data[1].data + adc_data[1].last_data)/2<350){
				PWM_SetDutyRatio(&pwm2[1],0);
				PWM_SetDutyRatio(&pwm2[2],0.7);
				PWM_SetDutyRatio(&pwm2[3],0);
				PWM_SetDutyRatio(&pwm2[4],0.7);
			}
	}
			if((adc_data[2].data + adc_data[2].last_data)/2<400){
			PWM_SetDutyRatio(&pwm2[1],0);
			PWM_SetDutyRatio(&pwm2[2],0);
			PWM_SetDutyRatio(&pwm2[3],0);
			PWM_SetDutyRatio(&pwm2[4],0);
		}

		
		
		
		
	 }

}


		/* You can write your own code here */
		/* USER CODE BEGIN */
		//HAL_GPIO_TogglePin(GPIOB,GPIO_PIN_8);
		/* USER CODE END */
		delay(5);
	}
